#include <SPI.h> 
#include <EEPROM.h>
#include <Streaming.h>
#include <SabertoothSimplified.h>
#include <TinyGPS.h>
#include <Wire.h>
#include <HMC5883L.h>
#include <EasyTransfer.h>
#include <Ultrasonic.h>
#include <Servo.h> 

EasyTransfer ET; 
//#define TRIGGER_PIN1  39
//#define ECHO_PIN1     41
//#define TRIGGER_PIN2  43
//#define ECHO_PIN2     45
//#define TRIGGER_PIN3  47
//#define ECHO_PIN3     49

#define TRIGGER_PIN1  49
#define ECHO_PIN1     47
#define TRIGGER_PIN2  45
#define ECHO_PIN2     43
#define TRIGGER_PIN3  41
#define ECHO_PIN3     39

//Ultrasonic ultrasonic1(TRIGGER_PIN1, ECHO_PIN1);
//Ultrasonic ultrasonic2(TRIGGER_PIN2, ECHO_PIN2);
Ultrasonic ultrasonic3(TRIGGER_PIN3, ECHO_PIN3);

volatile float cmMsec1, cmMsec2, cmMsec3;

Servo trigger;
#define tiltstop 44

struct RECEIVE_DATA_STRUCTURE{
  //put your variable definitions here for the data you want to receive
  //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO
  int Dfront;
  int Dleft;
  int Dright;
};

RECEIVE_DATA_STRUCTURE mydata;

#define RC
#define EnableGPS
#define EnableNav
//#define EnableUS
//#define XB

#define GunTilt1 4 
#define GunTilt2 7
#define GunTilt3 6
#define GunTilt4 5
#define laser 46
#define deadzone 10



String inputString = "";         // a string to hold incoming data
boolean stringComplete = false;  // whether the string is complete

// Store our compass as a variable.
HMC5883L compass;
int error = 0;

SabertoothSimplified ST(Serial2); // We'll name the Sabertooth object ST.

TinyGPS gps;

#define Holdatwaypoint 5000

#define VSYNC A2// INT0
#define MAX7456SELECT 10//ss
#define MAXReset A3
//MAX7456 opcodes
#define DMM_reg   0x04
#define DMAH_reg  0x05
#define DMAL_reg  0x06
#define DMDI_reg  0x07
#define VM0_reg   0x00
#define VM1_reg   0x01

//MAX7456 commands
#define CLEAR_display 0x04
#define CLEAR_display_vert 0x06
#define END_string 0xff

// with PAL
// all VM0_reg commands need bit 6 set
#define ENABLE_display 0x48
#define ENABLE_display_vert 0x4c
#define MAX7456_reset 0x42
#define DISABLE_display 0x40

#define WHITE_level_80 0x03
#define WHITE_level_90 0x02
#define WHITE_level_100 0x01
#define WHITE_level_120 0x00

// with PAL
#define MAX_screen_size 480
#define MAX_screen_rows 16

volatile byte screen_buffer[MAX_screen_size];

volatile byte writeOK;
volatile byte valid_string;
volatile byte save_screen;
volatile int  incomingByte;
volatile int  count;




bool newData = false;
unsigned long chars;
unsigned short sentences, failed;

int currentway=0;
float currentlat;
float currentlon;

boolean HomePosStored = false;
int GPSFIX = 0;
int SATNUM = 0;
#define distanceError 0.00002
#define highSpeed 100     
#define lowSpeed 50   
#define lowSpeedturn 20   
float wayPoints[20];
float HomePoints[2];
float heading;
float flat, flon, x2lat, x2lon;
void printFloat(double f, int digits = 2);
int noumWaypoints=0;
int Waycount=0;
float latitude, longitude, lastlat, lastlong;
float headingcompass;
int x4;
#define left 40
#define right 140
int year;
byte month, day, hour, minutes, second, hundredths;



int commands[10];
String SendCommands[10];

int mins[6];
int maxs[6];

int x;
int y;
int xmin = 100;
int ymin = 100;
int xmax = 0;
int ymax = 0;




int ycontroloffset=4;
int xcontroloffset=-8;

int xmins;
int xmaxs;
int ymins;
int ymaxs;


// a maximum of eight servo objects can be created 
int RXCH[6];
int RXCHval[6];
int RXCHmin[6];
int RXCHmax[6];
volatile int RXSG[6];
int RXOK[6];
int PWMSG[6];

//const int slaveSelectPin = 10;
const int strobe = 9;
const int headlights = 8;
const int aux = 10;

float Dist =0;
float DistLeft =0;
float DistRight =0;
struct Timing {
  unsigned long current;
  unsigned long US;
  unsigned long previous;
  unsigned long lastxbee;
  int delta;
} 
time = { 
  0, 0, 0, 0};

void setup() 
{ 
//  pinMode(TRIGGER_PIN1, OUTPUT);
//  pinMode(ECHO_PIN1,INPUT);
//  pinMode(TRIGGER_PIN2, OUTPUT);
//  pinMode(ECHO_PIN2,INPUT);
//  pinMode(TRIGGER_PIN3, OUTPUT);
//  pinMode(ECHO_PIN3,INPUT);  
  
  inputString.reserve(200);
  pinMode(tiltstop,INPUT);
  digitalWrite(tiltstop,HIGH);
  SPI.begin(); 
  Wire.begin(); // Start the I2C interface.
  Serial.begin(115200);
  Serial1.begin(9600);
  Serial2.begin(9600);
  Serial3.begin(4800);

  trigger.attach(2);

  ET.begin(details(mydata), &Serial1);

  ST.drive(0); // The Sabertooth won't act on mixed mode until
  ST.turn(0);  // it has received power levels for BOTH throttle and turning, since it
  // mixes the two together to get diff-drive power levels for both motors.
  // So, we set both to zero initially.

  pinMode(GunTilt1,OUTPUT);
  pinMode(GunTilt2,OUTPUT);
  pinMode(GunTilt3,OUTPUT);
  pinMode(GunTilt4,OUTPUT);
  pinMode(laser,OUTPUT);  

  Serial.println("*** LOADING ***");

  pinMode(MAX7456SELECT,OUTPUT);
  digitalWrite(MAX7456SELECT,HIGH); //disable device
  pinMode(VSYNC, INPUT);
  pinMode(MAXReset,OUTPUT);


  compass = HMC5883L(); // Construct a new HMC5883 compass.
  error = compass.SetScale(1.3); // Set the scale of the compass.
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous  

    wayPoints[0] = 51.4288889066323;
  wayPoints[1] = 0.555189102888107;
  wayPoints[2] = 51.4288044544777;
  wayPoints[3] = 0.555033534765244;
  wayPoints[4] = 51.4289056298127;
  wayPoints[5] = 0.5549356341362;
  wayPoints[6] = 51.4289240253041;
  wayPoints[7] = 0.55505096912384;
  noumWaypoints = 8;

  maxs[0] = 0;
  maxs[1] = 10;
  maxs[2] = 20;  
  maxs[3] = 30;
  maxs[4] = 40;
  maxs[5] = 50;
  mins[0] = 5;
  mins[1] = 15;
  mins[2] = 25;  
  mins[3] = 35;
  mins[4] = 45;
  mins[5] = 55;
  xmins = 60;
  xmaxs = 65;
  ymins = 70;
  ymaxs = 75;

  //  xmin = EEPROM.read(xmins);
  //  xmax = EEPROM.read(xmaxs);
  //  ymin = EEPROM.read(ymins);
  //  xmax = EEPROM.read(ymaxs);  
#ifdef RC
  RXCHmin[4] = 0;
  RXCHmin[5] = 0;
  RXCHmax[4] = 511;
  RXCHmax[5] = 511;
  RXCHmin[6] = 0;
  RXCHmax[6] = 511;
  RXCHmin[3] = 0;
  RXCHmax[3] = 511;
  RXCHmin[1] = 0;
  RXCHmax[1] = 511;  
  RXCHmin[2] = 0;
  RXCHmax[2] = 511;
#endif

  RXCH[1] = 22; //Aux 
  RXCH[2] = 24; //Gear 
  RXCH[3] = 26; //Rudd 
  RXCH[4] = 28; //Elev 
  RXCH[5] = 30; //Aile 
  RXCH[6] = 32; //Throttle
  for (int i = 1; i < 7; i++){
    pinMode(RXCH[i], INPUT);
  }
  // set the slaveSelectPin as an output:  
  pinMode (strobe,OUTPUT);  
  pinMode (headlights,OUTPUT);  
  pinMode (aux,OUTPUT);  
  // initialize SPI:  
  digitalWrite(headlights,HIGH);
  digitalWrite(strobe,HIGH);
  digitalWrite(aux,HIGH);  
  writeOK = false;
  valid_string = false;
  save_screen = false;
  incomingByte = 0;
  count = 0;
  pinMode(MAXReset,OUTPUT);
  delay(5000);
  digitalWrite(MAXReset,HIGH);

} 


void loop() { 
  if(ET.receiveData()){
  }
//  if(digitalRead(tiltstop) == LOW){
//    Serial.println("TiltSTOP!");
//    digitalWrite(GunTilt1,LOW);        
//    digitalWrite(GunTilt2,LOW);
//    digitalWrite(GunTilt3,LOW);    
//    digitalWrite(GunTilt4,LOW); 
//  }

  time.current = millis();
  time.delta = (int) (time.current - time.previous);
  time.previous = time.current;

#ifdef RC

  for (int i = 1; i < 7; i++){						    //for each of the 6 channels:
    RXSG[i] = pulseIn(RXCH[i], 254, 20000);				    //read the receiver signal
    if (RXSG[i] == 0) {
      RXSG[i] = RXOK[i];
    } 
    else {
      RXOK[i] = RXSG[i];
    }    //if the signal is good then use it, else use the previous signal
    PWMSG[i] = map(RXSG[i], 1000, 2000, 0, 511);				//substitute the 254 values to a value between 0 and 511
    constrain (PWMSG[i], 0, 511);	
    RXCHval[i] = PWMSG[i];

    Serial.print("CH");
    Serial.print(i);
    Serial.print(": ");
    Serial.print(RXCHval[i]);

    if (PWMSG[i] < RXCHmin[i]){
      RXCHmin[i] = PWMSG[i];
      EEPROM.write(mins[i], PWMSG[i]);
    }
    if (PWMSG[i] > RXCHmax[i]){
      RXCHmax[i] = PWMSG[i];
      EEPROM.write(maxs[i], PWMSG[i]);
    }
    //    delay(5);
  }
  Serial.println("");
  if (RXCHval[1] > 175 && RXCHval[1] < 185){
    digitalWrite(strobe,LOW); 
    digitalWrite(headlights,LOW);    
  }
  else if (RXCHval[1] > 385 && RXCHval[1] < 395){
    digitalWrite(strobe,HIGH); 
    digitalWrite(headlights,HIGH);

  }
  //  else if (RXCHval[1] > 280 && RXCHval[1] < 295){
  //    digitalWrite(strobe,HIGH); 
  //    digitalWrite(headlights,HIGH);
  //
  //  }

#ifdef EnableGPS


  if (newData)
  {
    float flat, flon;
    unsigned long age;
    gps.f_get_position(&flat, &flon, &age);
    unsigned long fix_age, time1, date, speed, course;

    float falt = gps.f_altitude(); // +/- altitude in meters
    float fc = gps.f_course(); // course in degrees
    float fk = gps.f_speed_knots(); // speed in knots
    float fmph = gps.f_speed_mph(); // speed in miles/hr


    gps.crack_datetime(&year, &month, &day, &hour, &minutes, &second, &hundredths, &fix_age);

    flat = TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat;
    flon = TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon;

    latitude = flat; 
    longitude = flon;

    Serial.print("LAT=");
    Serial.print(flat, 6);
    Serial.print(" LON=");
    Serial.print(flon, 6);
    Serial.print(" SAT=");
    Serial.print(gps.satellites() == TinyGPS::GPS_INVALID_SATELLITES ? 0 : gps.satellites());
    Serial.print(" PREC=");
    Serial.print(gps.hdop() == TinyGPS::GPS_INVALID_HDOP ? 0 : gps.hdop());
    Serial.print(" Alt=");
    Serial.print(falt);
    Serial.print(" COURSE=");
    Serial.print(fc);
    Serial.print(" MPH=");
    Serial.print(fmph);    

    if(!HomePosStored){
      HomePoints[0] = flat;
      HomePoints[1] = flon;
      HomePosStored = true;
    }

    newData = false;
  }

  gps.stats(&chars, &sentences, &failed);
  Serial.print(" CHARS=");
  Serial.print(chars);
  Serial.print(" SENTENCES=");
  Serial.print(sentences);
  Serial.print(" CSUM ERR=");
  Serial.println(failed);

#endif
  Processcomp();

  //  if (RXCHval[2] > (RXCHmax[2] / 2)){

  if (RXCHval[2] < (RXCHmax[2] / 2)){
    if (RXCHval[1] > 280 && RXCHval[1] < 290){
      digitalWrite(strobe,HIGH); 
      if(HomePosStored){
        ReadWay();
      }
      else{
        HomePoints[0] = latitude;
        HomePoints[1] = longitude; 
      }

    }
    else {
#if !defined(EnableUS)
      Dist = 60;
#endif
#ifdef EnableUS
//#define TRIGGER_PIN1  39
//#define ECHO_PIN1     41

//    digitalWrite(TRIGGER_PIN3, LOW);
//    delayMicroseconds(2);
//    digitalWrite(TRIGGER_PIN3, HIGH);
//    delayMicroseconds(10);
//    digitalWrite(TRIGGER_PIN3, LOW);
//    long micro = pulseIn(ECHO_PIN3, HIGH);
//    float Fdist = micro / 27.6233 / 2.0;
//    Dist = Fdist;


  long microsec3 = ultrasonic3.timing();
  cmMsec3 = ultrasonic3.convert(microsec3, Ultrasonic::CM);  
  Dist = cmMsec3;
  
      Serial.println("%%%%%%%%%%%%%%%%%%%%%%");
      Serial.println(Dist);
      Serial.println("%%%%%%%%%%%%%%%%%%%%%%");
#endif      
      
      if(Dist < 50 && RXCHval[4] < (RXCHmax[4] / 2)){
        Serial.println("STOPSTOPSTOPSTOP");
        ST.drive(0);
        ST.turn(0);
      }
      else{
        Serial.println("******************* DRIVE *******************");

        //Range -127 Reverse to 127 forward
        ST.drive(constrain(map(RXCHval[4],RXCHmin[4],RXCHmax[4],-127,127),-127,127));
        ST.turn(constrain(map(RXCHval[5],RXCHmin[5],RXCHmax[5],-127,127),-127,127));
      }
      Serial.print("Power: ");
      Serial.println(constrain(map(RXCHval[4],RXCHmin[4],RXCHmax[4],-127,127),-127,127));
      Serial.print("Turn: ");
      Serial.println(constrain(map(RXCHval[5],RXCHmin[5],RXCHmax[5],-127,127),-127,127));
    }
    digitalWrite(laser,LOW); 
    digitalWrite(GunTilt1,LOW);        
    digitalWrite(GunTilt2,LOW);
    digitalWrite(GunTilt3,LOW);    
    digitalWrite(GunTilt4,LOW);          
  }
  else{
    digitalWrite(laser,HIGH);
    Serial.print("PanPower:");
    Serial.print(RXCHval[4]);
    Serial.print(" TiltPower:");
    Serial.println(RXCHval[5]);
    //#define GunTilt1 4 
    //#define GunTilt2 7
    //#define GunTilt3 6
    //#define GunTilt4 5

    if (RXCHval[4] < ((RXCHmax[4] / 2)-deadzone)){
      Serial.print("TiltUP! --- CH4VAL: ");
      Serial.println(RXCHval[4]);
      digitalWrite(GunTilt1,LOW);    
      digitalWrite(GunTilt4,LOW);
      digitalWrite(GunTilt3,HIGH);    
      digitalWrite(GunTilt2,HIGH);          
    } 
    else if (RXCHval[4] > ((RXCHmax[4] / 2)+deadzone)){
      if(digitalRead(tiltstop) == HIGH){
        Serial.print("TiltDOWN! --- CH4VAL: ");
        Serial.println(RXCHval[4]);
        digitalWrite(GunTilt2,LOW);    
        digitalWrite(GunTilt3,LOW);
        digitalWrite(GunTilt4,HIGH);    
        digitalWrite(GunTilt1,HIGH);
      }
      else{
        Serial.println("TiltSTOP!");
        digitalWrite(GunTilt1,LOW);        
        digitalWrite(GunTilt2,LOW);
        digitalWrite(GunTilt3,LOW);    
        digitalWrite(GunTilt4,LOW);          

      }  
    }
    else{
      Serial.println("TiltSTOP!");
      digitalWrite(GunTilt1,LOW);        
      digitalWrite(GunTilt2,LOW);
      digitalWrite(GunTilt3,LOW);    
      digitalWrite(GunTilt4,LOW);         
    }

    ST.turn(constrain(map(RXCHval[5],RXCHmin[5],RXCHmax[5],-50,50),-50,50));   
    trigger.write(constrain(map(RXCHval[3],RXCHmin[3],RXCHmax[3],0,180),0,180));
  }




#endif
}


void STOP(){
  Serial.println("STOPSTOPSTOPSTOP");
  ST.drive(0);
  ST.turn(0);

}



String SplitString(String Data){

  int i, count; 
  for (i=0, count=0; Data[i]; i++) 
    count += (Data[i] == ':'); 

  for(int i =0; i < count;i++){
    int firstpos = Data.indexOf(":");
    commands[i] = convint(Data.substring(0,firstpos));
    Data = Data.substring(firstpos+1,Data.length());
  }
}

int convint(String Data){
  char carray2[Data.length() +1];
  Data.toCharArray(carray2,sizeof(carray2));
  return atoi(carray2); 
}


void serialEvent3(){
  //  for (unsigned long start = millis(); millis() - start < 1000;)
  //  {
  while (Serial3.available())
  {
    char c = Serial3.read();
    // Serial.write(c); // uncomment this line if you want to see the GPS data flowing
    if (gps.encode(c)) // Did a new valid sentence come in?
      newData = true;
  }
  //  }

}

void ReadWay(){
  if (noumWaypoints > 0 && SATNUM > 0){
    if(currentway == noumWaypoints){
      STOP();
      //      delay(5000); 
    }
    else{
      currentlat = wayPoints[currentway];
      currentlon = wayPoints[currentway+1];

      Navigate(currentlat,currentlon);

      if(((currentlon-longitude)<distanceError && (currentlon-longitude)>-distanceError) && ((currentlat-latitude)<distanceError && (currentlat-latitude)>-distanceError))
      {
        STOP();
        delay(Holdatwaypoint);
        currentway ++;  
        currentway ++;  
      }
    }
  }

}

void Navigate(float cflat,float cflon  ){

  float flat1 =latitude; 
  float flon1 =longitude;
  Serial.println(flat1,DEC);
  Serial.println(flon1,DEC);
  float dist_calc=0;
  float dist_calc2=0;
  float diflat=0;
  float diflon=0;
  x2lat=cflat;  
  x2lon=cflon;
  //Calculate distance from current location to waypoint
  diflat=radians(x2lat-flat1);  //Must be done in radians
  flat1=radians(flat1);    //convert current latitude to radians
  x2lat=radians(x2lat);  //convert waypoint latitude to radians
  diflon=radians((x2lon)-(flon1));   //subtract and convert longitudes to radians
  dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
  dist_calc2= cos(flat1);
  dist_calc2*=cos(x2lat);
  dist_calc2*=sin(diflon/2.0);                                       
  dist_calc2*=sin(diflon/2.0);
  dist_calc +=dist_calc2;
  dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));
  dist_calc*=6371000.0; //Converting to meters
  Serial.println("distance");
  Serial.println(dist_calc);    //print the distance in meters

  flon1 = radians(flon1);  //also must be done in radians
  x2lon = radians(x2lon);  //also must be done in radians
  heading = atan2(sin(x2lon-flon1)*cos(x2lat),cos(flat1)*sin(x2lat)-sin(flat1)*cos(x2lat)*cos(x2lon-flon1)),2*3.1415926535;
  heading = heading*180/3.1415926535;  // convert from radians to degrees
  int head =heading; //make it a integer now
  if(head<0){
    heading+=360;   //if the heading is negative then add 360 to make it positive
  }
  Serial.println("heading:");
  Serial.println(heading);   // print the heading.


  int newheading = headingcompass +180;
  x4=newheading-heading;   //getting the difference of our current heading to our needed bearing
  Serial << "Compass Heading: " << newheading << endl;
  Serial << "Heading Difference: " << x4 <<endl;

  int turn;
  // Now to work out which way to turn

    if(x4>=-180){

    if(x4<=-15){
      Serial.println("Turning RIGHT");
      if(DistRight < 20){
        if(Dist < 20){
          ST.drive(lowSpeed);
          ST.turn(-lowSpeedturn); 
        }
        else{
          ST.drive(lowSpeed);
          ST.turn(0); 
        }
      }
      else{
        ST.drive(lowSpeed);
        ST.turn(lowSpeedturn); 
      }
      //      motorRight(lowSpeed);
      return;
    }
  }
  if(x4<-180){
    Serial.println("Turning LEFT");
    if(DistLeft <20){
      if(Dist <20){
        ST.drive(lowSpeed);
        ST.turn(lowSpeedturn);
      }
      else{
        ST.drive(lowSpeed);
        ST.turn(0); 
      }  
    }
    else{
      ST.drive(lowSpeed);
      ST.turn(-lowSpeedturn);
    }
    //    motorLeft(lowSpeed);
    return;
  }
  if(x4>=15){

    if(x4<180){
      Serial.println("Turning LEFT");
      if(DistLeft <20){
        if(Dist <20){
          ST.drive(lowSpeed);
          ST.turn(lowSpeedturn);
        }
        else{
          ST.drive(lowSpeed);
          ST.turn(0); 
        }  
      }
      else{
        ST.drive(lowSpeed);
        ST.turn(-lowSpeedturn);
      }
      //      motorLeft(lowSpeed);
      return;

    }

  }

  if(x4>=180){
    Serial.println("Turning LEFT");
    if(DistLeft <20){
      if(Dist <20){
        ST.drive(lowSpeed);
        ST.turn(lowSpeedturn);
      }
      else{
        ST.drive(lowSpeed);
        ST.turn(0); 
      }  
    }
    else{
      ST.drive(lowSpeed);
      ST.turn(-lowSpeedturn);
    }
    //    motorLeft(lowSpeed);
    return;
  }

  if (x4 >= -15 || x4 <= 15){
    if(dist_calc > 15){
      Serial.println("Go Straight FAST");
      ST.drive(highSpeed);
      ST.turn(0);
      //      motorForward(feed);
    }
    else{
      Serial.println("Go Straight SLOW");      
      ST.drive(highSpeed);
      ST.turn(0);
      //      motorForward(lowSpeed);   //go "straight"
    }
  }
}
void Processcomp(){
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();

  // Values are accessed like so:
  int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  heading = atan2(scaled.YAxis, scaled.XAxis);
  float declinationAngle = 0.0457;
  heading += declinationAngle;

  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;

  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;

  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI;
  headingcompass = headingDegrees;
  Serial.print("Heading: ");
  Serial.println(headingcompass);
}


//////////////////////////////////////////////////////////////
byte convert_ascii (int character)
{
  // for some reason the MAX7456 does not follow ascii letter
  // placement, so you have to have this odd lookup table
  // todo... create an eeprom table that matches ascii
  // and burn to the MAX7456

  byte lookup_char;

  if (character == 32)
    lookup_char = 0x00; // blank space
  else if (character == 48)
    lookup_char = 0x0a; // 0
  else if ((character > 48) && (character < 58))
    lookup_char = (character - 48); // 1-9
  else if ((character > 64) && (character < 90))
    lookup_char = (character - 54); // A-Z
  else if ((character > 96) && (character < 123))
    lookup_char = (character - 60); // a-z
  else if (character == 34)
    lookup_char = 0x48; // "
  else if (character == 39)
    lookup_char = 0x46; // '
  else if (character == 40)
    lookup_char = 0x3f; // (
  else if (character == 41)
    lookup_char = 0x40; // )
  else if (character == 44)
    lookup_char = 0x45; // ,
  else if (character == 45)
    lookup_char = 0x49; // -
  else if (character == 46)
    lookup_char = 0x41; // .
  else if (character == 47)
    lookup_char = 0x47; // /
  else if (character == 58)
    lookup_char = 0x44; // :
  else if (character == 59)
    lookup_char = 0x43; // ;
  else if (character == 60)
    lookup_char = 0x4a; // <
  else if (character == 62)
    lookup_char = 0x4b; // >
  else if (character == 63)
    lookup_char = 0x42; // ?
  else
    lookup_char = 0x00; // out of range, blank space

  return (lookup_char);

}

//////////////////////////////////////////////////////////////
void write_new_screen(String Data)
{
  char carray2[Data.length() +1];
  Data.toCharArray(carray2,sizeof(carray2));

  for (int x = 0;x< Data.length();x ++){
    incomingByte = carray2[x];
    //    Serial.print(incomingByte);
    screen_buffer[x] = convert_ascii(incomingByte);  
  }
  //  Serial.println("");
  int x, local_count;
  byte char_address_hi, char_address_lo;
  byte screen_char;

  local_count = Data.length();

  char_address_hi = 0;
  char_address_lo = 0;
  //Serial.println("write_new_screen");  

  // clear the screen
  digitalWrite(MAX7456SELECT,LOW);
  SPI.transfer(DMM_reg);
  SPI.transfer(CLEAR_display);
  digitalWrite(MAX7456SELECT,HIGH);

  // disable display
  digitalWrite(MAX7456SELECT,LOW);
  SPI.transfer(VM0_reg);
  SPI.transfer(DISABLE_display);

  SPI.transfer(DMM_reg); //dmm
  //SPI.transfer(0x21); //16 bit trans background
  SPI.transfer(0x01); //16 bit trans w/o background

  SPI.transfer(DMAH_reg); // set start address high
  SPI.transfer(char_address_hi);

  SPI.transfer(DMAL_reg); // set start address low
  SPI.transfer(char_address_lo);

  x = 0;
  while(local_count) // write out full screen
  {
    screen_char = screen_buffer[x];
    SPI.transfer(DMDI_reg);
    SPI.transfer(screen_char);
    x++;
    local_count--;
  }

  SPI.transfer(DMDI_reg);
  SPI.transfer(END_string);

  SPI.transfer(VM0_reg); // turn on screen next vertical
  SPI.transfer(ENABLE_display_vert);
  digitalWrite(MAX7456SELECT,HIGH);
}






